/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 *
 * @author hpadmin
 */
public class Forklift extends Manipulator{
    public DigitalInput ina;
    public DigitalInput inb;
    private Encoder enc;
    public PIDController pid;
    private static final double kp = 0.1;
    private static final double ki = 0.0;
    private static final double kd = 0.0;
    private DriverStation ds;

    /**
     * Initializes Forklift with top switch on port 1, bottom switch on port
     * 2 and motor on PWM 3
     */
    public Forklift(){
        super(1, 2, 3);

        // init encoder
        ina = new DigitalInput(3);
        inb = new DigitalInput(4);
        enc = new Encoder(ina, inb);

        // enc as a pid source returns distance (not velocity)
        enc.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);

        // distance per pulse for enc.
        // diameter of spool is 3.6cm
        // pi*d/n = 3.14*3.6cm/360pulses = 0.031415926535898cm/pulse
        // = 0.012368475in/pulse
        enc.setDistancePerPulse(0.012368475014133);

        // create PID controller
        pid = new PIDController(kp, ki, kd, enc, motor);
        // bounds on pid input (encoder distance)
        // TODO: Set input bounds for forklift encoder.  Currently set to conservative value
        pid.setInputRange(0.0, 36.0);
        //bounds on pid output (motor)
        pid.setOutputRange(-1.0, 1.0);

        ds = DriverStation.getInstance();
    }

    /**
     * Use the bottom limit switch to reset the encoder.  Lowers the lift until
     * the bottom limit switch is depressed then raises until it is released
     * then immediately resets the encoder
     */
    public void resetEncoder(){
        System.out.println("Resetting encoder");
        if(!retractedSwitch.get()){
            System.out.println("ERROR: manipulator is not at bottom");
        }
        System.out.print("Moving up to release bottom limit switch...");
        while(retractedSwitch.get()){
            setVelocity(.25);
            Timer.delay(0.003);
        }
        setVelocity(0);
        System.out.println("Released");
        enc.reset();
        System.out.println("Encoder is reset");
    }

    /**
     * Sets velocity of forklift for manual control.
     * @param val desired velocity of forklift
     */
    public void setVelocity(double val){
        if(pid.isEnable()){
            System.out.print("ERROR: PID enabled; You cannot manually control.");
            return;
        }
        else{
            super.setVelocity(val);
            ds.setDigitalOut(2, retractedSwitch.get());
            ds.setDigitalOut(3, extendedSwitch.get());
        }
    }

    /**
     * Enables the pid.
     */
    public void pidEnable(){
        pid.reset();
        pid.enable();
        enc.start();
    }
    
    /**
     * Disables the pid
     */
    public void pidDisable(){
        pid.disable();
        enc.stop();
        motor.set(0.0);
    }

    /**
     * Sets the setpoint for the pid. Must be preceeded by pidEnable.
     * @param dist target height for the lift.
     */
    public void setDistance(double dist){
        pid.setSetpoint(dist);
    }

    public double getDistance(){
        return enc.getDistance();
    }
    public int getEncoderPulseCount(){
        return enc.get();
    }

    public void checkLimits(){
        if(extendedSwitch.get()){
            pid.disable();
            setVelocity(0.0);
            System.out.println("HIT LIMIT SWITCH: SHUTTING DOWN FORKLIFT");
        }
    }
}
